//----------------------------------------------------------------
#include "Boid.h"
//----------------------------------------------------------------
Boid::Boid(SVector2D kPos, float fMaxSpeed, float fMaxForce, Scene* pkScene)
{
	m_kAcc = SVector2D(0,0);
	m_kVel = SVector2D(RandInt(-1,1),RandInt(-1,1));
	m_kPos = kPos;
	m_fR = 2.0;
	m_fMaxSpeed = fMaxSpeed;
	m_fMaxForce = fMaxForce;

	m_pkGraphic = new Box();
	m_pkGraphic->setDim(5,5);
	m_pkGraphic->setColor(255,0,0);
	pkScene->addEntity(m_pkGraphic);
}
//----------------------------------------------------------------
void Boid::Run(vector<Boid> kBoids)
{
	Flock(kBoids);
	Update();
	Borders();

	m_pkGraphic->setPos(m_kPos.x, m_kPos.y);
}
//----------------------------------------------------------------
void Boid::Flock(vector<Boid> kBoids)
{
	SVector2D kSeparation = Separate(kBoids);
	SVector2D kAligment = Align(kBoids);
	SVector2D kCohesion = Cohesion(kBoids);

	kSeparation *= 1.5;
	kAligment *= 1.0;
	kCohesion *= 1.0;

	m_kAcc += kSeparation;
	m_kAcc += kAligment;
	m_kAcc += kCohesion;
}
//----------------------------------------------------------------
void Boid::Update() 
{
	m_kVel += m_kAcc;
	Vec2DClamp(m_kVel, m_fMaxSpeed);

	m_kPos += m_kVel;

	m_kAcc *= 0;
}
//----------------------------------------------------------------
void Boid::Seek(SVector2D kTarget)
{
	m_kAcc += Steer(kTarget,false);
}
//----------------------------------------------------------------
void Boid::Arrive(SVector2D kTarget)
{
	m_kAcc += Steer(kTarget,true);
}
//----------------------------------------------------------------
SVector2D Boid::Steer(SVector2D kTarget, bool bSlowDown)
{
	SVector2D kSteer;
	SVector2D kDesired = kTarget - m_kPos;
	float fD = Vec2DLength(kDesired);
	if (fD > 0)
	{
		Vec2DNormalize(kDesired);
		if ((bSlowDown) && (fD < 100.0f))
			kDesired *= (m_fMaxSpeed * (fD / 100.0f));
		else 
			kDesired *= m_fMaxSpeed;

		kSteer = (kDesired-m_kVel);
		Vec2DClamp(kSteer, m_fMaxForce);  // Limit to maximum steering force
	} 
	else
	{
		kSteer = SVector2D(0,0);
	}

	return kSteer;
}
//----------------------------------------------------------------
/*
void render() {
// Draw a triangle rotated in the direction of velocity
float theta = vel.heading2D() + radians(90);
fill(175);
stroke(0);
pushMatrix();
translate(loc.x,loc.y);
rotate(theta);
beginShape(TRIANGLES);
vertex(0, -r*2);
vertex(-r, r*2);
vertex(r, r*2);
endShape();
popMatrix();
}
*/
//----------------------------------------------------------------
void Boid::Borders()
{
	if (m_kPos.x < -400)
		m_kPos.x = 400;
	if (m_kPos.y < -300)
		m_kPos.y = 300;
	if (m_kPos.x > 400)
		m_kPos.x = -400;
	if (m_kPos.y > 300)
		m_kPos.y = -300;
}
//----------------------------------------------------------------
SVector2D Boid::Separate (vector<Boid> kBoids)
{
	float fDesiredSeparation = 25.0f;
	SVector2D kSteer = SVector2D(0,0);
	int iCount = 0;

	for (int i = 0 ; i < kBoids.size(); i++)
	{
		float fD = Vec2DDistance(m_kPos,kBoids[i].m_kPos);
		if ((fD > 0) && (fD < fDesiredSeparation))
		{
			SVector2D kDiff = m_kPos - kBoids[i].m_kPos;
			Vec2DNormalize(kDiff);
			kDiff /= fD;
			kSteer += kDiff;
			iCount++;
		}
	}

	if (iCount > 0)
	{
		kSteer /= iCount;
	}

	if (Vec2DLength(kSteer) > 0)
	{
		Vec2DNormalize(kSteer);
		kSteer *= m_fMaxSpeed;
		kSteer -= m_kVel;
		Vec2DClamp(kSteer, m_fMaxForce);
	}

	return kSteer;
}
//----------------------------------------------------------------
SVector2D Boid::Align (vector<Boid> kBoids)
{
	float kNeighbordist = 50.0;
	SVector2D kSteer = SVector2D(0,0);
	int iCount = 0;

	for (int i = 0 ; i < kBoids.size(); i++)
	{
		float fD = Vec2DDistance(m_kPos, kBoids[i].m_kPos);
		if ((fD > 0) && (fD < kNeighbordist))
		{
			kSteer += kBoids[i].m_kVel;
			iCount++;
		}
	}

	if (iCount > 0) 
	{
		kSteer /= iCount;
	}

	if (Vec2DLength(kSteer) > 0)
	{
		Vec2DNormalize(kSteer);
		kSteer *= m_fMaxSpeed;
		kSteer -= m_kVel;
		Vec2DClamp(kSteer, m_fMaxForce);
	}

	return kSteer;
}
//----------------------------------------------------------------
SVector2D Boid::Cohesion (vector<Boid> kBoids)
{
	float fNeighbordist = 50.0;
	SVector2D kSum = SVector2D(0,0);
	int iCount = 0;

	for (int i = 0 ; i < kBoids.size(); i++)
	{
		float fD = Vec2DDistance(m_kPos, kBoids[i].m_kPos);
		if ((fD > 0) && (fD < fNeighbordist))
		{
			kSum += kBoids[i].m_kPos;
			iCount++;
		}
	}

	if (iCount > 0)
	{
		kSum /= iCount;
		return Steer(kSum,false);
	}

	return kSum;
}